
/**
 * Copyright: (C) 2011 RobotCub Consortium
 * @author Gabriel Baud-Bovy
 * @author Mike Ciaraldi
 * @author Dalia De Santis
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 * Wrapper class for Motion Trackers 
 */

#ifdef _WINDOWS
#include <tchar.h>
#include <windows.h>
#include "PDI.h"
#endif


#include <iostream>
#include <string>
using namespace std;

#include "Tracker.h"
#include "ConstantTracker.h"
#include "CircleTracker.h"

#ifdef _WINDOWS
#include "PolhemusTracker.h"
#else
#include "PiTracker.h"
#include "PingPong.h"
#include "PiPeek.h"
#include "LinuxPolhemusTracker.h"
#endif

#include "TrackerDevice.h"

TrackerDevice::TrackerDevice(){
	acquisitionType=TRACKER_POLLING;
}

bool TrackerDevice::open(yarp::os::Searchable& config) {

	// select input device
	string type = string(config.find("type").asString());
	if(type.compare("polhemus")== 0 || type.compare("Polhemus")== 0)
	{
#ifdef _WINDOWS
		PolhemusTracker *pTracker = new PolhemusTracker();
#else
		LinuxPolhemusTracker *pTracker = new LinuxPolhemusTracker();
#endif
		pTracker->setRate(120.0);
		tracker = pTracker;
		printf("---> Polhemus tracker device selected (120 Hz)\n");
	}
	else if(type.compare("circle")== 0 || type.compare("Circle")== 0)
	{
		tracker = new CircleTracker();
		printf("---> Circle tracker device selected\n");
	}
	else
	{
		printf("ERROR: non existing device selected!\n");
		return false;
	}

	// check for input mode
	type = config.find("mode").asString();
	if(type.compare("streaming")== 0  ||  type.compare("Streaming")==0){
		acquisitionType=TRACKER_STREAMING;
		printf("---> Streaming acquisition mode\n");
	} else {
		acquisitionType=TRACKER_POLLING;
		printf("---> Polling acquisition mode\n");
	}

	// check for input calibration parameters
	// default = [0,0,0,0,0,0];
	calibParam = new double[6];
	if(config.check("calibration"))
	{	
		Bottle &calib = config.findGroup("calibration");

		if (calib.size() != 7)
		{
			printf("ERROR IN READING CALIBRATION PARAMETERS\n");
			printf("---> Setting default calibration param\n");
			
			for (int i=0; i<6; i++)
				calibParam[i] = 0;
		}
		else
		{
			for (int i =1; i<7; i++)
				calibParam[i-1] = calib.get(i).asDouble();

			printf("---> Calibration parameter loaded\n");
		}
	}
	else
	{
		printf("---> Setting default calibration param\n");
		for (int i=0;i<6;i++)
			calibParam[i] = 0;
	}

	// open connection with tracker
	if(tracker->open()) {
		printf("---> Tracker opened\n");
	} else {
		printf("Error: %s\n",tracker->getErrorInfo());
		return false;
	};

	// start streaming
	switch(acquisitionType){
	case TRACKER_STREAMING:
		if(tracker->start()) {
			printf("---> Streaming started\n");
		} else {
			printf("Error: %s\n",tracker->getErrorInfo());
			return false;		
		};
		break;
	};

  return true;
}

bool TrackerDevice::close() {
	// stop streaming
	switch(acquisitionType){
	case TRACKER_STREAMING:
	case TRACKER_STREAMING_LATEST:
		if(!tracker->stop()) {
			printf("Error: %s\n",tracker->getErrorInfo());
			return false;		
		}
		break;
	}

	// close connection with tracker
	tracker->close();
	delete tracker;
	return true;
}

// Default
bool TrackerDevice::read(yarp::sig::Vector &out) {
  out.resize(6);

  float data[6];

	switch(acquisitionType){
	case TRACKER_STREAMING:
		// update Polhemus tracker buffer
		if(!tracker->read()) {
			printf("Streaming read error: %s\n",tracker->getErrorInfo());
			return false;
		}
		// get data from Polhemus tracker buffer
		for(int i=0;i<6;i++) tracker->getData(0,i,data[i]);
		if(!tracker->nextFrame()){
			printf("Streaming next frame error: %s\n",tracker->getErrorInfo());
			return false;
		};
	    break;
	case TRACKER_POLLING:
		if(!tracker->poll(data)) {
			printf("Polling error: %s\n",tracker->getErrorInfo());
			return false;
		}
		break;
	}

	// convert to double
	for(int i=0;i<6;i++) out[i] = (double)data[i]+calibParam[i];

	return true;
}

// Default
bool TrackerDevice::getChannels(int *nc){
  if (!nc) return false;
  *nc = 6;
  return true;
}

bool TrackerDevice::calibrate(int ch, double v){
  return true;
}

char *TrackerDevice::getErrorInfo(void){
	return tracker->getErrorInfo();
}
